/**
 * @file Mechanization.hpp
 * @brief INS Mechanization Algorithms
 * @author Linfu Wei (ghowoght@qq.com)
 * @version 1.0
 * @date 2021-06-05
 * 
 * @copyright Copyright (c) 2021  WHU-EIS
 * 
 */
#ifndef _MECHANIZATION_HPP
#define _MECHANIZATION_HPP

#include <iostream>
#include <eigen3/Eigen/Dense>
#include "utils.hpp"

namespace Nav{

class Mechanization{
private:
    StatePtr state_;
    State state_last_;

    ImuData data_now_;
    ImuData data_last_;

    double dt_; // 更新时间间隔
    double Rm_; // 子午圈曲率半径
    double Rn_; // 卯酉圈曲率半径
    Eigen::Vector3d rotvec_b_; // b系的等效旋转向量
    Eigen::Vector3d rotvec_n_; // n系的等效旋转向量
    Eigen::Vector3d rotvec_e_; // e系的等效旋转向量
    Eigen::Vector3d w_ie_; // n系下的角速度
    Eigen::Vector3d w_en_; // n系下的角速度
    Eigen::Vector3d mid_pos_; // t-0.5时刻的位置
    Eigen::Vector3d mid_vel_; // t-0.5时刻的速度
    Eigen::Vector3d dv_last_; // 上一次的速度增量，包括比力、重力、哥氏力引起的增量
    Eigen::Quaterniond q_ne_;
    Eigen::Quaterniond q_nn_; 

public:
    Mechanization() = default;
    Mechanization(const StatePtr& state, ImuData& data):state_(state), data_now_(data), data_last_(data){
        // quat: n->b
        state_->q_nb = Eigen::AngleAxisd(state_->att[YAW], Eigen::Vector3d::UnitZ())
                    * Eigen::AngleAxisd(state_->att[PIT], Eigen::Vector3d::UnitY())
                    * Eigen::AngleAxisd(state_->att[ROL], Eigen::Vector3d::UnitX()) ;

        Pos2Quat(state_->pos, state_->q_ne);
          
        state_last_ = *state_;
        dv_last_ << 0, 0, 0;
    }

    void update(ImuData& data){
        data_now_ = data;
        dt_ = data_now_.timestamp - state_->timestamp;
        
        // 更新速度
        update_vel();

        // 更新位置
        update_pos();

        // 更新姿态
        update_att();

        state_->timestamp = data_now_.timestamp;
        data_last_ = data_now_;
        
        state_last_ = *state_;
    }

private:
    void update_vel(){
        // t-1时刻n系下的角速度矢量
        w_ie_ << OMEGA_E * cos(state_->pos[LAT]),
                 0,
                -OMEGA_E * sin(state_->pos[LAT]);
        getRmRn(state_->pos[LAT], Rm_, Rn_);
        w_en_ <<  state_->vel[E] / (Rn_ + state_->pos[ALT]),
                 -state_->vel[N] / (Rm_ + state_->pos[ALT]),
                 -state_->vel[E] / (Rn_ + state_->pos[ALT]) * tan(state_->pos[LAT]);
        // 计算从t-1到t-0.5的n系旋转矢量
        rotvec_n_ = (w_ie_ + w_en_) * dt_ * 0.5;
        // t-0.5时刻的n系变换四元数
        Eigen::Vector4d temp;
        temp <<  sin(0.5 * rotvec_n_.norm()) / (0.5 * rotvec_n_.norm()) * 0.5 * rotvec_n_, 
                 cos(0.5 * rotvec_n_.norm());
        Eigen::Quaterniond q_nn(temp);
        // t-1到t-0.5的e系变化的旋转矢量
        rotvec_e_ << 0, 0, -OMEGA_E * dt_ * 0.5;
        // t-0.5时刻的e系变换四元数
        temp << -sin(0.5 * rotvec_e_.norm()) / (0.5 * rotvec_e_.norm()) * 0.5 * rotvec_e_, 
                 cos(0.5 * rotvec_e_.norm());
        Eigen::Quaterniond q_ee(temp);
        // t-1时刻的q_ne
        Pos2Quat(state_->pos, state_->q_ne);

        /* 位置外推：计算t-0.5时刻的位置 */
        state_->q_ne = q_ee * state_->q_ne * q_nn;        
        Quat2Pos(state_->q_ne, mid_pos_);
        mid_pos_[ALT] = state_->pos[ALT] - 0.5 * state_->vel[D] * dt_;

        // t-0.5时刻的角速度矢量
        w_ie_ << OMEGA_E * cos(mid_pos_[LAT]),
                 0,
                -OMEGA_E * sin(mid_pos_[LAT]);
        getRmRn(mid_pos_[LAT], Rm_, Rn_);
        w_en_ <<  state_->vel[E] / (Rn_ + mid_pos_[ALT]),
                 -state_->vel[N] / (Rm_ + mid_pos_[ALT]),
                 -state_->vel[E] / (Rn_ + mid_pos_[ALT]) * tan(mid_pos_[LAT]);

        // t-1到t的n系旋转矢量
        rotvec_n_ = (w_ie_ + w_en_) * dt_;

        // 由比力引起的速度增量
        Eigen::Vector3d v_fk_b, v_fk_n;
        v_fk_b  = data_now_.accel_increm 
                + 0.5 * getAntiSymmetricMatrix(data_now_.gyro_increm) * data_now_.accel_increm
                + (getAntiSymmetricMatrix(data_last_.gyro_increm) * data_now_.accel_increm
                + getAntiSymmetricMatrix(data_last_.accel_increm) * data_now_.gyro_increm) / 12.0;

        v_fk_n = (Eigen::Matrix3d::Identity() - 0.5 * getAntiSymmetricMatrix(rotvec_n_)) * state_->q_nb.toRotationMatrix() * v_fk_b;

        // 由哥氏力和重力引起的速度增量
        Eigen::Vector3d g_n = getG(mid_pos_);
        mid_vel_ = state_->vel + 0.5 * dv_last_;
        Eigen::Vector3d v_g_cor = (g_n - getAntiSymmetricMatrix(2 * w_ie_ + w_en_) * mid_vel_) * dt_;
        // 保存速度增量
        dv_last_ = v_fk_n + v_g_cor;
         
        state_->vel = state_->vel + v_fk_n + v_g_cor;

        // std::cout.precision(12);
        // std::cout << state_->vel.transpose() << std::endl;
    }

    void update_pos(){
        // 更新高度
        state_->pos[ALT] = state_->pos[ALT] - 0.5 * (state_last_.vel[D] + state_->vel[D]) * dt_;
        // 更新纬度
        double alt_mean = 0.5 * (state_last_.pos[ALT] + state_->pos[ALT]);
        state_->pos[LAT] = state_->pos[LAT] + (state_->vel[N] + state_last_.vel[N]) / 2 / (Rm_ + alt_mean) * dt_;
        // 更新经度
        double lat_mean = 0.5 * (state_last_.pos[LAT] + state_->pos[LAT]);
        getRmRn(lat_mean, Rm_, Rn_);
        state_->pos[LON] = state_->pos[LON] + (state_->vel[E] + state_last_.vel[E]) / 2 / (Rn_ + alt_mean) / cos(lat_mean) * dt_;
        Pos2Quat(state_->pos, state_->q_ne);


        // // 从导航系到地固系的姿态转换四元数包含经纬度信息 
        // // q(nk,ek)=q(nk,nk-1)*q(nk-1, ek-1)*q(ek-1,ek)    
        // // 速度内插
        // mid_vel_ = (state_->vel + state_last_.vel) * 0.5;
        // // 重新计算w_en
        // w_en_ <<  mid_vel_[E] / (Rn_ + mid_pos_[ALT]),
        //          -mid_vel_[N] / (Rm_ + mid_pos_[ALT]),
        //          -mid_vel_[E] / (Rn_ + mid_pos_[ALT]) * tan(mid_pos_[LAT]);
        // // t-1到t的n系旋转矢量
        // rotvec_n_ = (w_ie_ + w_en_) * dt_;
        // // 计算t到t-1时刻n系的变换四元数
        // Eigen::Vector4d temp;
        // temp <<  sin(0.5 * rotvec_n_.norm()) / (0.5 * rotvec_n_.norm()) * 0.5 * rotvec_n_, 
        //          cos(0.5 * rotvec_n_.norm());
        // Eigen::Quaterniond q_nn(temp);
        // // 计算t-1到t时刻e系的变换四元数
        // rotvec_e_ << 0, 0, OMEGA_E * dt_;
        // temp << -sin(0.5 * rotvec_e_.norm()) / (0.5 * rotvec_e_.norm()) * 0.5 * rotvec_e_, 
        //          cos(0.5 * rotvec_e_.norm());
        // Eigen::Quaterniond q_ee(temp);
        // // 计算t-1时刻n系到e系的变换四元数
        // // Pos2Quat(state_->pos, state_->q_ne);
        // // 更新t时刻的q_ne
        // state_->q_ne = q_ee * state_->q_ne * q_nn;
        // state_->q_ne.normalize();
        // // 提取经纬度信息
        // Quat2Pos(state_->q_ne, state_->pos);

        // // 更新高度
        // state_->pos[ALT] = state_->pos[ALT] - mid_vel_[D] * dt_;

        
        // std::cout.precision(12);
        // std::cout << state_->pos.transpose() << std::endl;
    }

    void update_att(){
        rotvec_b_   = data_now_.gyro_increm
                    + getAntiSymmetricMatrix(data_last_.gyro_increm) * data_now_.gyro_increm / 12.0;
        
        Eigen::Vector4d temp;
        temp << sin(0.5 * rotvec_b_.norm()) / (0.5 * rotvec_b_.norm()) * 0.5 * rotvec_b_, 
                cos(0.5 * rotvec_b_.norm());
        Eigen::Quaterniond q_bb(temp);

        temp << -sin(0.5 * rotvec_n_.norm()) / (0.5 * rotvec_n_.norm()) * 0.5 * rotvec_n_,
                 cos(0.5 * rotvec_n_.norm());
        Eigen::Quaterniond q_nn(temp);

        state_->q_nb = q_nn * state_->q_nb * q_bb;
        state_->q_nb.normalize();

        DCM2Euler(state_->q_nb.toRotationMatrix(), state_->att);
    }
};

}
#endif